#include "../../../XMLTools.h"
#include "../../../MathTools.h"

#include "IMUSensorMeasurement.h"

using namespace MMM;

IMUSensorMeasurement::IMUSensorMeasurement(float timestep, const Eigen::Vector3f &acceleration, SensorMeasurementType type) :
    InterpolatableSensorMeasurement(timestep, type),
    acceleration(acceleration)
{
}

SensorMeasurementPtr IMUSensorMeasurement::clone() {
    return clone(timestep);
}

bool IMUSensorMeasurement::equals(SensorMeasurementPtr sensorMeasurement) {
    IMUSensorMeasurementPtr ptr = boost::dynamic_pointer_cast<IMUSensorMeasurement>(sensorMeasurement);
    if (ptr) {
        return acceleration == ptr->acceleration;
    }
    return false;
}

IMUSensorMeasurementPtr IMUSensorMeasurement::clone(float newTimestep) {
    IMUSensorMeasurementPtr clonedSensorMeasurement(new IMUSensorMeasurement(newTimestep, acceleration, type));
    return clonedSensorMeasurement;
}

void IMUSensorMeasurement::appendMeasurementDataXML(RapidXMLWriterNodePtr measurementNode) {
    measurementNode->append_node("Acceleration")->append_data_node(XML::toString(acceleration));
}

IMUSensorMeasurementPtr IMUSensorMeasurement::interpolate(IMUSensorMeasurementPtr other, float timestep, SensorMeasurementType type) {
    Eigen::Vector3f interpolatedAcceleration = Math::linearInterpolation(this->acceleration, this->timestep, other->acceleration, other->timestep, timestep);
    IMUSensorMeasurementPtr interpolatedSensorMeasurement(new IMUSensorMeasurement(timestep, interpolatedAcceleration, type));
    return interpolatedSensorMeasurement;
}

Eigen::Vector3f IMUSensorMeasurement::getAcceleration() {
    return acceleration;
}
